#include <string>
#include <fstream>

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/GetMap.h>
#include <tf/transform_listener.h>

#include "search/rescue/map.hpp"
#include "search/rescue/rescue.hpp"
#include "search/rescue/sim.hpp"

#define RIGHT 0
#define UP 1
#define LEFT 2
#define DOWN 3

#define UNKNOWN -1

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

extern unsigned int MaxPeople;

struct pt {
	pt() : x(0), y(0) {}
	pt(double x, double y) : x(x), y(y) {}
	double dist(double _x, double _y) {
		double dx = x - _x;
		double dy = y - _y;
		return sqrt(dx*dx + dy*dy);
	}
	double x,y;
};

struct rescueData {
	Map map;
	unsigned int currentNode;
	unsigned int homeNode;
};

struct peoplePt {
	peoplePt() : found(false), index(-1) {}
	peoplePt(const pt& p, int index, bool found) : p(p), found(found), index(index) {}
	pt p;
	bool found;
	int index;
};

/* ROS node related stuff */
ros::Subscriber mapMetadataSubscriber;
nav_msgs::GetMap::Request  req;
nav_msgs::GetMap::Response resp;
move_base_msgs::MoveBaseGoal lastIssuedGoal;
int seqNumber = 0;

/* application related stuff */
double duration = 0;
int numberOfHindsightSamples = 32;
int expectedSquareMeters = 400;
double expectedAvgVelocity = 1;
double clearDistanceFromObstacles = 0.1;

pt homeLocation;
double mapResolution;
double coarseResolution = 1;
std::vector<peoplePt> peoplePts;
std::vector<int> allPeopleOnGraph;
std::vector<bool> foundPeopleOnGraph;

rescueData constructCoarseMap(const std::vector<signed char> occupancy, int height,
			      int width, double x_off, double y_off,
			      const std::vector<double> &position,
			      double clearDist = 0.1, double occupiedThreshold = 0.75) {

	ROS_INFO("Constructing course map");

	ROS_INFO("map offsets %f %f", x_off, y_off);
	Map myMap;
//	double edgeWeight = (double)skip_size * Mapresolution;
	int skip_size = coarseResolution / mapResolution;
	int nearestCurrentNodeId = 0;
	double distToNearestCurrentNode = std::numeric_limits<double>::infinity();

	int nearestHomeNodeId = 0;
	double distToNearestHomeNode = std::numeric_limits<double>::infinity();
//	int clearNodes = (clearDist / mapResolution);

	pt currentLocation(position[0], position[1]);

	int prev_y = 0;
	std::vector<int> ids((width*height), -1);
	int currentNode = 0;

	int home_row = (int)((homeLocation.x - x_off) / mapResolution);
	int home_col = (int)((homeLocation.y - y_off) / mapResolution);

	int start_x = (int)(fmod((home_row / skip_size), skip_size) * skip_size);
	int start_y = (int)(fmod((home_col / skip_size), skip_size) * skip_size);

	unsigned int i = start_x + start_y * width;

	for( ; i < occupancy.size(); i+=skip_size) {
		int x = i % width;
		int y = (int)(i / width);
		if(prev_y != y) {
			int new_y = prev_y + skip_size;
			if(new_y >= height) break;

			i = new_y * width;
			y = new_y;
			prev_y = y;
		}

		if(occupancy[i] > occupiedThreshold || //occupied or unknown
		   occupancy[i] == UNKNOWN) continue;

		assert(ids[(x + y * width)] == -1);
		ids[(x + y * width)] = currentNode;

		Map::Node &n = myMap.addnode(currentNode);
		n.pt.x = (double)x * mapResolution + x_off;
		n.pt.y = (double)y * mapResolution + y_off;
		n.r = 2;

		assert(!isinf(n.pt.x));
		assert(!isinf(n.pt.y));

		double d = currentLocation.dist(n.pt.x, n.pt.y);
		if(d < distToNearestCurrentNode) {
			distToNearestCurrentNode = d;
			nearestCurrentNodeId = currentNode;
		}

		d = homeLocation.dist(n.pt.x, n.pt.y);
		if(d < distToNearestHomeNode) {
			distToNearestHomeNode = d;
			nearestHomeNodeId = currentNode;
		}

		currentNode++;
	}
	ROS_INFO("Coarse Graph contains %d Nodes", currentNode);

	prev_y = 0;
	int currentEdge = 0;

	i = start_x + start_y * width;
	for( ; i < occupancy.size(); i+=skip_size) {
		double x = i % width;
		double y = (int)(i / width);
		if(prev_y != y) {
			int new_y = prev_y + skip_size;
			if(new_y >= height) break;

			i = new_y * width;
			y = new_y;
			prev_y = y;
		}

		if(occupancy[i] > occupiedThreshold || //occupied or unknown
		   occupancy[i] == UNKNOWN) continue;

		bool connected = true;
		int x_mut = x;
		int y_mut = y;
		int id1 = ids[(x + y * width)];

		assert(id1 >= 0);

//		assert(	fabs(myMap.nodes[id1].pt.x - (x*mapResolution)) <= 0.00001);
//		assert(	fabs(myMap.nodes[id1].pt.y - (y*mapResolution)) <= 0.00001);

/* consider the possibility of a connection to the right of this node */
		connected = true;
		x_mut = x;
		y_mut = y;
		for(int j = 0; j <= skip_size; j++) {
			x_mut = x + j;
			if(x_mut >= width ||
			   occupancy[x_mut + y * width] > occupiedThreshold) {
				connected = false;
				break;
			}
		}

		if(connected && occupancy[x_mut + y * width] != UNKNOWN) {
			int id2 = ids[(x_mut + y * width)];
			assert(id2 >= 0);
//			assert(fabs(myMap.nodes[id2].pt.x - (x_mut*mapResolution)) <= 0.00001);
//			assert(fabs(myMap.nodes[id2].pt.y - (y*mapResolution)) <= 0.00001);

			assert(currentEdge >= 0);
			myMap.addedge(currentEdge, id1, id2);
			myMap.nodes[id1].e[RIGHT] = currentEdge;
			myMap.nodes[id2].e[LEFT] = currentEdge;
			currentEdge++;
		}
		else if(x_mut >= width || occupancy[x_mut + y * width] == UNKNOWN) {
			myMap.nodes[id1].e[RIGHT] = Map::Node::MayEdge;
		}
		else {
			myMap.nodes[id1].e[RIGHT] = Map::Node::NoEdge;
		}

/* consider the possibility of a connection up from this node */
		connected = true;
		x_mut = x;
		y_mut = y;
		for(int j = 0; j <= skip_size; j++) {
			y_mut = y + j;
			if(y_mut >= height || occupancy[x + y_mut * width] > occupiedThreshold) {
				connected = false;
				break;
			}
		}

		if(connected && occupancy[x + y_mut * width] != UNKNOWN) {
			int id2 = ids[(x + y_mut * width)];
			assert(id2 >= 0);
//			assert(fabs(myMap.nodes[id2].pt.x - (x*mapResolution)) <= 0.00001);
//			assert(fabs(myMap.nodes[id2].pt.y - (y_mut*mapResolution)) <= 0.00001);

			assert(currentEdge >= 0);
			myMap.addedge(currentEdge, id1, id2);
			myMap.nodes[id1].e[UP] = currentEdge;
			myMap.nodes[id2].e[DOWN] = currentEdge;
			currentEdge++;
		}
		else if(y_mut >= height || occupancy[x + y_mut * width] == UNKNOWN) {
			myMap.nodes[id1].e[UP] = Map::Node::MayEdge;
		}
		else {
			myMap.nodes[id1].e[UP] = Map::Node::NoEdge;
		}

/* the two above cases of up and right will build all the connections that we really need, however,
   we still need to mark nodes as growable or not growable so we'll do that below.

   What is to the left of this node */
		connected = true;
		x_mut = x;
		y_mut = y;
		for(int j = 0; j <= skip_size; j++) {
			x_mut = x - j;
			if(x_mut < 0 || occupancy[x_mut + y * width] > occupiedThreshold) {
				connected = false;
				break;
			}
		}

		if(connected && occupancy[x_mut + y * width] != UNKNOWN) {
			/* don't add an edge because we'll consider this edge implicity as an up edge */
		}
		else if(x_mut < 0 || occupancy[x_mut + y * width] == UNKNOWN) {
			myMap.nodes[id1].e[LEFT] = Map::Node::MayEdge;
		}
		else {
			myMap.nodes[id1].e[LEFT] = Map::Node::NoEdge;
		}

/* What's below this node? */
		connected = true;
		x_mut = x;
		y_mut = y;
		for(int j = 0; j <= skip_size; j++) {
			y_mut = y - j;
			if(y_mut < 0 || occupancy[x + y_mut * width] > occupiedThreshold) {
				connected = false;
				break;
			}
		}
		if(connected && occupancy[x + y_mut * width] != UNKNOWN) {
			/* don't add an edge because we'll consider this edge implicity as an up edge */
		}
		else if(y_mut < 0 || occupancy[x + y_mut * width] == UNKNOWN) {
			myMap.nodes[id1].e[DOWN] = Map::Node::MayEdge;
		}
		else {
			myMap.nodes[id1].e[DOWN] = Map::Node::NoEdge;
		}
	}

	for(unsigned int i = 0; i < myMap.nodes.size(); i++) {
		assert(myMap.nodes[i].id >= 0);
//		bool connected = false;
//		for(int j = 0; j < Map::Ndirs; j++)
//			if(myMap.nodes[i].e[j] >= 0)
//				connected = true;
//		if(!connected && (int)i != nearestCurrentNodeId && (int)i != nearestHomeNodeId)
//			myMap.nodes[i].id = -1;
//		else
		if(myMap.nodes[i].growable())
			myMap.nodes[i].c = Image::green;
	}


	std::vector<Map::Node> nodes = myMap.nodes;
	allPeopleOnGraph.clear();
	foundPeopleOnGraph.clear();
	double distanceThreshold = coarseResolution;
	for(unsigned int i = 0; i < peoplePts.size(); i++) {
/*	 	if(peoplePts[i].found) {
			peoplePts[i].index = i;
			allPeopleOnGraph.push_back(0);
			foundPeopleOnGraph.push_back(true);
		}
		else { */
		int index = -1;
		double minDist = std::numeric_limits<double>::infinity();
		for(unsigned int j = 0; j < nodes.size(); j++) {
			double dist = peoplePts[i].p.dist(nodes[j].pt.x, nodes[j].pt.y);
			if(dist < minDist) {
				index = j;
				minDist = dist;
			}
		}
		if(index >= 0 && minDist <= distanceThreshold) {
			peoplePts[i].index = index;
			allPeopleOnGraph.push_back(index);
			foundPeopleOnGraph.push_back(peoplePts[i].found);
			if(peoplePts[i].found) myMap.nodes[index].c = Color(255./255., 52./255., 179./255.);
			else myMap.nodes[index].c = Color(238./255., 121./255., 159./255.);
			myMap.nodes[index].r = 5;
		}
//		}
	}

	rescueData retVal;
	retVal.map = myMap;
	retVal.currentNode = nearestCurrentNodeId;
	retVal.homeNode = nearestHomeNodeId;

	return retVal;
}


const move_base_msgs::MoveBaseGoal getNextGoalPosition(Rescue rescue) {
	static bool finished = false;
	ROS_INFO("Calling Planner");

	double goalX = 0;
	double goalY = 0;

	int estimatedNumberOfNodes = expectedSquareMeters / coarseResolution;

	ROS_INFO("estimated number of nodes: %d", estimatedNumberOfNodes);

	Hindsight hindsight(numberOfHindsightSamples, estimatedNumberOfNodes, false);
	boost::optional<Op> operation = hindsight.plan(rescue);

	while(operation && operation->kind == Op::Report) {
		//print current location and request to report and await confirmation
		Map::Node &node = rescue.m.nodes[rescue.loc];

		int counter = 0;
		for(unsigned int i = 0; i < peoplePts.size(); i++)
			if(peoplePts[i].index == node.id) {
				peoplePts[i].found = true;
				counter++;
			}

		ROS_WARN("REPORT %d people at %f %f (%d)", counter, node.pt.x, node.pt.y, node.id);

		rescue.act(operation);
		operation = hindsight.plan(rescue);
	}
	if(operation) {
		if(operation->kind == Op::Move) {
			int nodeId = operation->value;
			Map::Node &node = rescue.m.nodes[nodeId];
			goalX = node.pt.x;
			goalY = node.pt.y;
		}
		else if(operation->kind == Op::None) {
			ROS_INFO("None action returned from Hindsight, rescue complete");
			exit(0);
		}
	}
	else {
		if(finished && rescue.m.nodes[rescue.home].id == rescue.m.nodes[rescue.loc].id) {
			ROS_INFO("Return home command already sent. Rescue complete.");
		 	exit(0);
		}
		finished = true;
		Map::Node &node = rescue.m.nodes[rescue.home];
		goalX = node.pt.x;
		goalY = node.pt.y;
		ROS_ERROR("Hindsight did not return an action! Returning to home!");
	}

	ROS_INFO("Got Next Action");

	lastIssuedGoal.target_pose.header.seq = seqNumber;
	lastIssuedGoal.target_pose.header.stamp = ros::Time::now();
	lastIssuedGoal.target_pose.header.frame_id = "/map";

	// These need to be set with the response from the planner
	lastIssuedGoal.target_pose.pose.position.x = goalX;
	lastIssuedGoal.target_pose.pose.position.y = goalY;

	lastIssuedGoal.target_pose.pose.position.z = 0;

	// probably make this be in line with the straightline trajectory?

	seqNumber++;
	return lastIssuedGoal;
}

rescueData processMap(const nav_msgs::OccupancyGrid& map,
					       const std::vector<double> &position) {
	ROS_INFO("Got Map Data");

	int width = map.info.width;
	int height = map.info.height;
	double x_off = map.info.origin.position.x;
	double y_off = map.info.origin.position.y;
	mapResolution = map.info.resolution;

	const std::vector<signed char> occupancyGrid = map.data;

	return constructCoarseMap(occupancyGrid, height, width, x_off, y_off, position, clearDistanceFromObstacles);
}

std::vector<double> getCurrentPositionInMap(const tf::TransformListener &tfListener) {
	std::vector<double> position;

	geometry_msgs::PointStamped baseLinkPoint;
	geometry_msgs::PointStamped mapPoint;

	mapPoint.header.frame_id = "/map";

	baseLinkPoint.header.frame_id = "/base_link";
	baseLinkPoint.header.stamp = ros::Time::now();

	baseLinkPoint.point.x = 0.0;
	baseLinkPoint.point.y = 0.0;
	baseLinkPoint.point.z = 0.0;

	std::string error;
	try {
		while(!tfListener.waitForTransform(baseLinkPoint.header.frame_id,
						   mapPoint.header.frame_id,
						   baseLinkPoint.header.stamp,
						   ros::Duration(1.0),
						   ros::Duration(0.01),
						   &error)) {
			baseLinkPoint.header.stamp = ros::Time::now();
			ROS_INFO("Waiting for transform between map and base_link: %s", error.c_str());
		}

		tfListener.transformPoint(mapPoint.header.frame_id, baseLinkPoint, mapPoint);
		position.push_back(mapPoint.point.x);
		position.push_back(mapPoint.point.y);
		position.push_back(mapPoint.point.z);
	}
	catch (tf::TransformException ex) {
		ROS_ERROR("%s",ex.what());
		ROS_ERROR("For testing just using 0,0,0");
		position.push_back(0);
		position.push_back(0);
		position.push_back(0);
//	exit(0);
	}
	return position;
}

bool parseArgs(int argc, char** argv) {
	for(int i = 1; i < argc; i++) {
		if(strcmp(argv[i],"-home") == 0) {
			i++;
			if(i >= argc) return false;
			int x = atof(argv[i]);
			i++;
			if(i >= argc) return false;
			int y = atof(argv[i]);
			homeLocation = pt(x,y);
		}
		else if(strcmp(argv[i],"-duration") == 0) {
			i++;
			if(i >= argc) return false;
			duration = atof(argv[i]);
		}
		else if(strcmp(argv[i],"-velocity") == 0) {
			i++;
			if(i >= argc) return false;
			expectedAvgVelocity = atof(argv[i]);
		}
		else if(strcmp(argv[i],"-samples") == 0) {
			i++;
			if(i >= argc) return false;
			numberOfHindsightSamples = atof(argv[i]);
		}
		else if(strcmp(argv[i],"-maxpeople") == 0) {
			i++;
			if(i >= argc) return false;
			MaxPeople = atoi(argv[i]);
		}
		else if(strcmp(argv[i],"-expectedBuildingSize") == 0) {
			i++;
			if(i >= argc) return false;
			double width = atof(argv[i]);
			i++;
			if(i >= argc) return false;
			double height = atof(argv[i]);
			expectedSquareMeters = width*height;
		}
		else if(strcmp(argv[i],"-coarseRes") == 0) {
			i++;
			if(i >= argc) return false;
			coarseResolution = atof(argv[i]);
		}
		else if(strcmp(argv[i],"-bufferDist") == 0) {
			i++;
			if(i >= argc) return false;
			clearDistanceFromObstacles = atof(argv[i]);
		}
		else if(strcmp(argv[i],"-peopleFile") == 0) {
			i++;
			if(i >= argc) return false;
			std::ifstream myfile(argv[i]);
			std::string line;

			if(myfile.is_open()) {
				while(myfile.good()) {
					getline(myfile, line);
					if(line.length() == 0) break;
					int index = line.find_first_of(" ");
					double x = atof(line.substr(0,index-1).c_str());
					double y = atof(line.substr(index).c_str());
					peoplePts.push_back(peoplePt(pt(x,y), -1, false));
					ROS_INFO("added person at (%f, %f)", x, y);
				}
			}
			else {
				ROS_ERROR("error reading file: %s\n\n", argv[i]);
				return false;
			}
		}

	}
	return duration > 0 && peoplePts.size() > 0;
}

void printHelp() {
	fprintf(stderr, "icaps_2013\n");
	fprintf(stderr, "required arguments\n");
	fprintf(stderr, "\t-duration \t\t number of seconds allowed for this search\n");
	fprintf(stderr, "\t-peopleFile \t\t file containing the x,y locations of injured people\n");

	fprintf(stderr, "optional arguments\n");
	fprintf(stderr, "\t-velocity \t\t expected average velocity of the robot (%f)\n", expectedAvgVelocity);
	fprintf(stderr, "\t-expectedBuildingSize \t expected max area square meters (%d)\n", expectedSquareMeters);
	fprintf(stderr, "\t-maxpeople \t\t expected number of people to be found (%u)\n", MaxPeople);
	fprintf(stderr, "\t-samples \t\t number of futures to generate (%d)\n", numberOfHindsightSamples);
	fprintf(stderr, "\t-home \t\t\t relative end location from start in meters (0 0)\n");
	fprintf(stderr, "\t-coarseRes \t\t the spacing between the coarse nodes in meters (%f)\n", coarseResolution);
	fprintf(stderr, "\t-bufferDist \t\t the mandatory free space distance around a node (%f)\n", clearDistanceFromObstacles);
}


void publishMarkers(const ros::Publisher &vis_pub) {
	visualization_msgs::MarkerArray markers;
	for(unsigned int i = 0; i < peoplePts.size(); i++) {
		visualization_msgs::Marker marker;
		marker.header.frame_id = "map";
		marker.header.stamp = ros::Time();
		marker.id = 0;
		marker.type = visualization_msgs::Marker::SPHERE;
		marker.action = visualization_msgs::Marker::ADD;
		marker.pose.position.x = peoplePts[i].p.x;
		marker.pose.position.y = peoplePts[i].p.y;
		marker.pose.position.z = 1;
		marker.pose.orientation.x = 0.0;
		marker.pose.orientation.y = 0.0;
		marker.pose.orientation.z = 0.0;
		marker.pose.orientation.w = 1.0;
		marker.scale.x = 1;
		marker.scale.y = 0.1;
		marker.scale.z = 0.1;
		marker.color.a = 1.0;
		marker.color.r = 1.0;
		marker.color.g = 0.0;
		marker.color.b = 0.0;
		marker.frame_locked = true;
		marker.lifetime = ros::Duration();

		markers.markers.push_back(marker);
	}
	vis_pub.publish( markers );
}

struct slidingAverage {
	slidingAverage(int windowSize)
		: index(0), size(windowSize),  values(windowSize, -1) {}

	void addValue(double value) {
		values[index] = value;
		index = (index + 1) % size;
	}

	double getAverage() const {
		if(values[0] < 0) return 1;
		double sum = 0;
		int i = 0;
		for( ; i < (int)size; i++) {
			if(values[i] < 0) break;
			sum += values[i];
		}

		return sum / (double)i;
	}

	int index;
	int size;
	std::vector<double> values;
};

int main(int argc, char** argv) {
	if(!parseArgs(argc, argv)) { printHelp(); return 0; }

	ros::init(argc, argv, "icaps_2013");
	ros::NodeHandle n;
	tf::TransformListener tfListener;

	ros::Publisher vis_pub = n.advertise<visualization_msgs::MarkerArray>("visualization_markers", 0);
	publishMarkers(vis_pub);


	ros::Time zeroTime(0);
	ros::Time startTime = ros::Time::now();

	while(startTime == zeroTime) startTime = ros::Time::now();

	ros::Time endTime = startTime + ros::Duration(duration);
	MoveBaseClient ac("move_base", true);

	//wait for the action server to come up
	while(!ac.waitForServer(ros::Duration(1.0))){
		ROS_INFO("Waiting for the move_base action server to come up");
	}

	ros::service::waitForService("dynamic_map", 10000);
	ROS_INFO("Algorithm Core");


	bool firstTime = true;
	slidingAverage looptimes(5);
	while(n.ok()) {
		ros::Time loopstart = ros::Time::now();

		ros::service::call("dynamic_map", req, resp); ROS_INFO("Got Map");

		if(!firstTime) ROS_INFO("MoveBase Result: %s", ac.getState().toString().c_str());

		std::vector<double> currentPosition;
		if(!firstTime && ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
			ROS_INFO("Goal Position achieved, using that as current position");
			currentPosition.push_back(lastIssuedGoal.target_pose.pose.position.x);
			currentPosition.push_back(lastIssuedGoal.target_pose.pose.position.y);
			currentPosition.push_back(lastIssuedGoal.target_pose.pose.position.z);
		} else {

			currentPosition = getCurrentPositionInMap(tfListener);



			int attempt = 0;

			move_base_msgs::MoveBaseGoal perturbedGoal = lastIssuedGoal;

			while(!firstTime && ac.getState() == actionlib::SimpleClientGoalState::ABORTED && attempt < 4) {
				ROS_WARN("TRYING TO PERTURB ENDPOINTS");
				switch(attempt) {
				case 0:
					perturbedGoal.target_pose.pose.position.x = lastIssuedGoal.target_pose.pose.position.x + coarseResolution * 0.5;
					perturbedGoal.target_pose.pose.position.y = lastIssuedGoal.target_pose.pose.position.y;
					// if(!(fabs(lastIssuedGoal.target_pose.pose.position.x - currentPosition[0]) <= 0.001 &&
					//    fabs(lastIssuedGoal.target_pose.pose.position.y - currentPosition[1]) <= 0.001)) {
					break;
				case 1:
					perturbedGoal.target_pose.pose.position.x = lastIssuedGoal.target_pose.pose.position.x - coarseResolution * 0.5;
					perturbedGoal.target_pose.pose.position.y = lastIssuedGoal.target_pose.pose.position.y;
					// if(!(fabs(lastIssuedGoal.target_pose.pose.position.x - currentPosition[0]) <= 0.001 &&
					//    fabs(lastIssuedGoal.target_pose.pose.position.y - currentPosition[1]) <= 0.001)) {
					break;
				case 2:
					perturbedGoal.target_pose.pose.position.x = lastIssuedGoal.target_pose.pose.position.x;
					perturbedGoal.target_pose.pose.position.y = lastIssuedGoal.target_pose.pose.position.y + coarseResolution * 0.5;
 					// if(!(fabs(lastIssuedGoal.target_pose.pose.position.x - currentPosition[0]) <= 0.001 &&
					//    fabs(lastIssuedGoal.target_pose.pose.position.y - currentPosition[1]) <= 0.001)) {
					break;
				default:
					perturbedGoal.target_pose.pose.position.x = lastIssuedGoal.target_pose.pose.position.x;
					perturbedGoal.target_pose.pose.position.y = lastIssuedGoal.target_pose.pose.position.y - coarseResolution * 0.5;
					// if(!(fabs(lastIssuedGoal.target_pose.pose.position.x - currentPosition[0]) <= 0.001 &&
					//    fabs(lastIssuedGoal.target_pose.pose.position.y - currentPosition[1]) <= 0.001)) {
					break;
				}
				attempt++;
				perturbedGoal.target_pose.header.stamp = ros::Time::now();
				perturbedGoal.target_pose.header.seq = seqNumber;
				perturbedGoal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
				seqNumber++;
				ac.sendGoal(perturbedGoal);
				ac.waitForResult();
			}
			lastIssuedGoal = perturbedGoal;
			ROS_INFO("Not sure where robot is, looking up position");
			currentPosition = getCurrentPositionInMap(tfListener);
		}

		ROS_INFO("Got Position (%f %f)", currentPosition[0], currentPosition[1]);

		rescueData results = processMap(resp.map, currentPosition);

		assert(results.map.nodes[results.currentNode].id >= 0);
		assert(results.map.nodes[results.homeNode].id >= 0);

		results.map.nodes[results.currentNode].r = 5;
		results.map.nodes[results.homeNode].r = 7;

		results.map.draw("incremental.eps");

		ROS_INFO("Map Processed - at node %d (%f, %f)", results.currentNode,
			 results.map.nodes[results.currentNode].pt.x,
			 results.map.nodes[results.currentNode].pt.y);

		ROS_INFO("Search Time remaining %f", (endTime - ros::Time::now()).toSec());

		double secondsRemaining = (endTime - ros::Time::now()).toSec();

//		double normalizedDeadline =  (secondsRemaining * expectedAvgVelocity) / coarseResolution;

		double currentAvg = looptimes.getAverage();

		ROS_INFO("Loop Time Current Avg: %f sec", currentAvg);

		double normalizedDeadline =  secondsRemaining / currentAvg;


		ROS_INFO("Deadline implies %f edge traversals remaining", normalizedDeadline);

		Rescue myRescue(results.map, allPeopleOnGraph, results.homeNode,
				results.currentNode, normalizedDeadline);
		myRescue.reported = foundPeopleOnGraph;

		move_base_msgs::MoveBaseGoal goal = getNextGoalPosition(myRescue);


		double angle = 0;
		if(currentPosition[0] > goal.target_pose.pose.position.x) angle = 0;
		else if(currentPosition[0] < goal.target_pose.pose.position.x) angle = M_PI;
		else if(currentPosition[1] > goal.target_pose.pose.position.y) angle = 0.5 * M_PI;
		else if(currentPosition[1] < goal.target_pose.pose.position.x) angle = 1.5 * M_PI;


//		double angle = atan2(currentPosition[1] - goal.target_pose.pose.position.y,
//				     currentPosition[0] - goal.target_pose.pose.position.x);

		goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(-angle);

		ROS_INFO("Got new goal position (%f,%f)", goal.target_pose.pose.position.x,
			 goal.target_pose.pose.position.y);

		ac.sendGoal(goal);

		ac.waitForResult();
		firstTime = false;

		ros::Time loopend = ros::Time::now();
		looptimes.addValue((loopend - loopstart).toSec());
	}
}
